#include "task_server_pkg/task_server.hpp"

namespace task_server_pkg
{
    TaskServer::TaskServer(const rclcpp::NodeOptions &options) : 
        nav2_util::LifecycleNode("task_server", "", options),
        planner_plugins_("map1")
    {
        RCLCPP_INFO(get_logger(), "TaskServer() create");
        declare_parameter("planner_plugins", "map2");

    }
    TaskServer::~TaskServer()
    {
    }
    nav2_util::CallbackReturn TaskServer::on_configure(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_configure");
        // 获取并打印参数
        if (get_parameter("planner_plugins", planner_plugins_)) {
            RCLCPP_INFO(get_logger(), "planner_plugins: %s", planner_plugins_.c_str());
        } else {
            RCLCPP_WARN(get_logger(), "Failed to get parameter 'planner_plugins'");
        }
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_activate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_activate");
        createBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_deactivate(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_deactivate");
        destroyBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_cleanup(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_cleanup");
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_shutdown(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(get_logger(), "on_shutdown");
        return nav2_util::CallbackReturn::SUCCESS;
    }
}